#include "BC_BodyPosTask.hpp"

template <typename T>
void  BodyPosTask<T>::task(ControlFSMData<T>& data, BalanceCtrlData<T>* ctrl_data, BCoutputData<T>* BCout, int leg){
    auto& seResult = data._stateEstimator->getResult();
    Vec3<T> delta =  seResult.rBody * (ctrl_data->pBody_des - seResult.position);
    Vec3<T> pDes = seResult.rBody * (ctrl_data->pFoot_des[leg] - seResult.position)  
                - data._quadruped->getHipLocation(leg);

    // only fix x and y
    Vec3<T> pRecord = pDes;
    // pDes = pDes + delta;
    pDes(2) = pRecord(2);
    Vec3<T> qDes;

    // convert
    computeLegAngle(*data._quadruped, pDes, &qDes);    
    BCout->qDes = qDes;
    BCout->qdDes = data._legController->datas[leg].J.inverse() * (seResult.rBody * ctrl_data->vBody_des);   
}


template class BodyPosTask<float>;
template class BodyPosTask<double>;
